#include "RDdata.h"
#include "i2c.h"
#include "usart.h"
#include "string.h"
#include "stdio.h"
#include "adc.h"
#include "tim.h"

#define Trace_Black_Mid 3320//3120 5401教室后面数据//3330
unsigned char chrTemp[30];

//寄存器相关操作第12行，第24行，第34行，巡线函数第57行，读取编码器第70行

/**************************************************************************
函数功能：将short数据存入寄存器
入口参数：寄存器地址，数据数组
返回  值：无
**************************************************************************/
void ShortToChar(short sData,unsigned char cData[])
{
	cData[0]=sData&0xff;
	cData[1]=sData>>8;
}

/**************************************************************************
函数功能：读取寄存器数据
入口参数：寄存器地址高一位，低本位
返回  值：读取数据值（尚未处理）
**************************************************************************/
short CharToShort(unsigned char cData[])
{
	return ((short)cData[1]<<8)|cData[0];
}

/**************************************************************************
函数功能：读取陀螺仪的值
入口参数：angle结构体
返回  值：无
**************************************************************************/
void JY901_RDDat(JY901_Angle *IMUx)
{
	
   HAL_I2C_Mem_Read(&hi2c1, 0x50<<1, AX, I2C_MEMADD_SIZE_8BIT, &chrTemp[0] , 24, 0xff);
	(*IMUx).a[0] = (float)CharToShort(&chrTemp[0])/32768*16;
	(*IMUx).a[1] = (float)CharToShort(&chrTemp[2])/32768*16;
	(*IMUx).a[2] = (float)CharToShort(&chrTemp[4])/32768*16;
	(*IMUx).w[0] = (float)CharToShort(&chrTemp[6])/32768*2000;
	(*IMUx).w[1] = (float)CharToShort(&chrTemp[8])/32768*2000;
	(*IMUx).w[2] = (float)CharToShort(&chrTemp[10])/32768*2000;
	(*IMUx).h[0] = CharToShort(&chrTemp[12]);
	(*IMUx).h[1] = CharToShort(&chrTemp[14]);
	(*IMUx).h[2] = CharToShort(&chrTemp[16]);
	(*IMUx).Angle[0] = (float)CharToShort(&chrTemp[18])/32768*180;
	(*IMUx).Angle[1] = (float)CharToShort(&chrTemp[20])/32768*180;
	(*IMUx).Angle[2] = (float)CharToShort(&chrTemp[22])/32768*180;
	
}

/**************************************************************************
函数功能：读取巡线偏差
入口参数：巡线结构体
返回  值：无
**************************************************************************/
void Trace_RDDat(Trace_Excursion *inf_excursion) {
	static uint16_t AD_Value[4];
	HAL_ADC_Start_DMA(&hadc1,(uint32_t*)AD_Value,4);
	inf_excursion->left=AD_Value[0];
	inf_excursion->mid=AD_Value[1];
	inf_excursion->right=AD_Value[2];
	//滤波
	inf_excursion->mid=KalmanFilter(inf_excursion->mid);
	//判断右偏，以及判断左偏。
	if(inf_excursion->left>=inf_excursion->right) {
		inf_excursion->Bias_Begin= inf_excursion->mid-Trace_Black_Mid;
	}else if(inf_excursion->left<inf_excursion->right) {
		inf_excursion->Bias_Begin= -inf_excursion->mid+Trace_Black_Mid;
	}
	if(inf_excursion->Bias_Begin<80&&inf_excursion->Bias_Begin>-80) inf_excursion->Trace_Bias=0;
	//限幅后不让数据直接从1000开始变换。
	else if(inf_excursion->Bias_Begin>=120) inf_excursion->Trace_Bias=inf_excursion->Bias_Begin-120;
	else if(inf_excursion->Bias_Begin<=-120) inf_excursion->Trace_Bias=inf_excursion->Bias_Begin+120;
	//inf_excursion->Trace_Bias=KalmanFilter(inf_excursion->Trace_Bias);
}

/**************************************************************************
函数功能：读取编码器
入口参数：左轮是TIM2，右轮是TIM3
返回  值：编码器的值
**************************************************************************/
int Read_Encoder(uint8_t TIMX)
{
	int Encoder_TIM;
	switch(TIMX)
	{
		case 2:  Encoder_TIM= -(short)TIM2 -> CNT;  TIM2 -> CNT=0;break;
		case 3:  Encoder_TIM=  (short)TIM3 -> CNT; TIM3 -> CNT=0;break;
		default: Encoder_TIM=0;
	}
	return Encoder_TIM;
}

uint16_t KalmanFilter(uint16_t inData)
{
	static float prevData = 0;
	static float p = 1, q = 0.001, r = 0.1, kGain = 0;

	p = p + q;
	kGain = p / ( p + r );
	inData = prevData + ( kGain * ( inData - prevData ) );
	p = ( 1 - kGain ) * p;
	prevData = inData;
	return inData;
}
